Skip to content

AC_Avoidance: Use closest fence point if Dijkstra starts outside fence #31827

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom
Open
aadarshagarwal1 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

AC_Avoidance: Use closest fence point if Dijkstra starts outside fence #31827

aadarshagarwal1 wants to merge 1 commit into from

Conversation

@aadarshagarwal1
Copy link
Contributor

@aadarshagarwal1 aadarshagarwal1 commented Dec 30, 2025

Resolves #31399

Bug Description

If a Rover (or Copter) using OADijkstra starts slightly outside an inclusion fence (e.g., due to GPS drift or loitering near the edge), calc_shortest_path fails.

  • update_visgraph returns true but generates 0 nodes because all paths to internal nodes intersect the fence.
  • The pathfinder returns DIJKSTRA_ERROR_OUT_OF_MEMORY (or general failure) and the vehicle cannot RTL or navigate back.

The Fix

This PR implements a "Recovery Mode" inside calc_shortest_path:

  1. Checks if _source_visgraph is empty after the initial update.
  2. If empty, iterates through _inclusion_polygon_pts to find the geometrically closest point on the fence boundary.
  3. Temporarily sets the path start node to this "Safe Boundary Point."
  4. Re-runs update_visgraph from this safe spot (which guarantees valid connections).
  5. Prepends the "Void -> Fence" leg to the final path so the vehicle drives to the fence first.

Verification

  • Compiled successfully for Rover.
  • Logic relies on Vector2f::closest_point for robust geometry handling.

@aadarshagarwal1
AC_Avoidance: Use closest fence point if Dijkstra starts outside fence
5d07eed 
Currently, if the vehicle starts outside the inclusion fence, update_visgraph returns an empty graph and pathfinding fails immediately. This adds a recovery mechanism: if the graph is empty, we find the closest point on the fence boundary, snap the start node to that safe point, and calculate the path from there.
@aadarshagarwal1
Copy link
Contributor Author

aadarshagarwal1 commented Jan 8, 2026

Hi @rmackay9 ,

This PR is ready for review (CI is passing). It solves the edge case where OADijkstra fails if the vehicle starts slightly outside an inclusion fence (common with GPS drift).

Summary of fix:

  1. Detects if the graph generation fails due to being outside the fence.
  2. Finds the nearest point on the fence boundary.
  3. Generates a path starting from that boundary point, allowing the vehicle to re-enter the safe zone and continue navigation.

I have tested this compilation locally. Could you please take a look when you have a moment? Thanks!

@1Blademaster
Copy link

1Blademaster commented Jan 21, 2026 •
edited
Loading

Not fully sure if this is in the scope of this PR specifically or issue #31399; but if I'm in ALTHOLD (for example) and I (Copter) move outside the inclusion fence or inside an exclusion fence, Dijkstra's cannot recalculate the path back into the safe flyable area. I've got OA_TYPE=2 and keep getting the message "Dijkstra: could not find path". I made a post on the discussion board about this issue and got linked to this PR

@rmackay9
Copy link
Contributor

rmackay9 commented Jan 21, 2026

Hi @1Blademaster, yes, the issue you're seeing is indeed the one that this PR aims to fix

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

Rover: Dijkastras should handle case where vehicle is started outside of fence

3 participants

@aadarshagarwal1 @1Blademaster @rmackay9